/*
 * ======== Standard MSP430 includes ========
 */
#include <msp430.h>

/*
 * ======== Grace related includes ========
 */
#include <ti/mcu/msp430/csl/CSL.h>

/*
 * ======== Standard includes ========
 */
#include <inttypes.h>
#include <setjmp.h>

/*
 * ======== Custom includes ========
 */
#include "threads.h"

/*
 * ======== Definitions ========
 */
#define NULL ((void*)0)
#define MAX_THREADS 10
// FIXME move to own header file
#define TRUE 1
#define FALSE 0

/*
 * ======== Variables ========
 */
static Thread_t gThreads[MAX_THREADS] = { 0 };
static uint16_t gCurrThread;
static uint8_t gOsRunning;

void ATOMIC_START(void) {
	__disable_interrupt();
}

void ATOMIC_END(void) {
	__enable_interrupt();
}

/* Do round robin
 * return: The next thread
 */
uint16_t roundRobin(void) {
	uint16_t nextThread = gCurrThread + 1;
	if (nextThread >= MAX_THREADS) {
		nextThread = 0;
	}

	if (gThreads[nextThread].state != THREAD_INVALID) {
		return nextThread;
	} else {
		// find next thread to run
		uint16_t i;
		for (i = nextThread; i < MAX_THREADS; ++i) {
			if (gThreads[i].state != THREAD_INVALID) {
				return i;
			}
		}
	}

	return 0;
}

void scheduler_startThread() {
	// start thread
	if (setjmp(gThreads[gCurrThread].context) != 0) {
		ATOMIC_END();
		gThreads[gCurrThread].run();
	}
}

/* Run next thread */
void scheduler_runNextThread() {
	ATOMIC_START();
	uint16_t nextThread = roundRobin();
	switch (gThreads[nextThread].state) {
	case THREAD_RUNNING:
		break; // already running
	case THREAD_READY:
		if (setjmp(gThreads[gCurrThread].context) == 0) { // saved current execution state (setjmp)

			// change state of the current running thread
			if (gThreads[gCurrThread].state == THREAD_RUNNING)
				gThreads[gCurrThread].state = THREAD_READY;

			// start the next thread
			gCurrThread = nextThread;
			gThreads[gCurrThread].state = THREAD_RUNNING;
			longjmp(gThreads[gCurrThread].context, 1);
		}
		break;
	default:
		break; // no thread to run -> sleep no atomic
	}
	ATOMIC_END();
}

/* Method for timer interrupt */
void schedulerInterrupt(void) {
	scheduler_runNextThread();
}

/* initialize os */
void init(void) {
	ATOMIC_START();
	gOsRunning = TRUE;

	gThreads[0] = createThread1(0);
	gCurrThread = gThreads[0].id;
	scheduler_startThread();

	gThreads[1] = createThread2(1);
	gCurrThread = gThreads[1].id;
	scheduler_startThread();

	gThreads[2] = createThread2(2);
	gCurrThread = gThreads[2].id;
	scheduler_startThread();

	// FIXME last thread never gets executed but all threads before

	/*
	gThreads[2] = createThread2(2);
	gCurrThread = gThreads[2].id;
	scheduler_startThread();
	*/
	ATOMIC_END();
}

/*
 *  ======== main ========
 */
int main(int argc, char *argv[]) {
	CSL_init(); // Activate Grace-generated configuration
	init();

	// keep running
	while (gOsRunning)
		;

	return 0;
}
